#!/usr/bin/env python3

import sys
import os
import shutil
import signal
import subprocess
import time
from PyQt5.QtWidgets import (
    QApplication, QWidget, QLabel, QPushButton, QVBoxLayout, QHBoxLayout, QScrollArea, QFrame,
    QLineEdit, QDialog, QDialogButtonBox, QMessageBox, QTextEdit, QListWidget, QListWidgetItem
)
from PyQt5.QtGui import QPixmap, QFont
from PyQt5.QtCore import Qt, QTimer, QProcess
import argparse
from pathlib import Path
import importlib.util
PATH_ROOT = Path(__file__).parents[4]

def parse_args():
    parser = argparse.ArgumentParser(description="Map Manager GUI")
    parser.add_argument("--debug", action="store_true", default=False, help="If tagged, play ros2 bag")
    args = parser.parse_args()
    return args

class RenameDialog(QDialog):
    def __init__(self, old_name, parent=None):
        super().__init__(parent)
        self.setWindowTitle("重命名文件夹")
        self.setFixedSize(300, 120)
        layout = QVBoxLayout()
        self.input = QLineEdit(self)
        self.input.setText(old_name)
        layout.addWidget(self.input)
        buttons = QDialogButtonBox(QDialogButtonBox.Ok | QDialogButtonBox.Cancel, self)
        buttons.accepted.connect(self.accept)
        buttons.rejected.connect(self.reject)
        layout.addWidget(buttons)
        self.setLayout(layout)

    def get_new_name(self):
        return self.input.text().strip()

class MappingDialog(QDialog):
    def __init__(self, parent=None):
        super().__init__(parent)
        self.setWindowTitle("建图终端")
        self.setFixedSize(700, 500)
        self.proc1 = None
        self.proc2 = None
        self.proc1_finished = False
        self.proc2_finished = False

        layout = QVBoxLayout()
        self.terminal1 = QTextEdit(self)
        self.terminal1.setReadOnly(True)
        layout.addWidget(QLabel("建图终端1:"))
        layout.addWidget(self.terminal1)

        self.terminal2 = QTextEdit(self)
        self.terminal2.setReadOnly(True)
        layout.addWidget(QLabel("DEBUG终端2:"))
        layout.addWidget(self.terminal2)

        self.stop_btn = QPushButton("停止", self)
        self.stop_btn.setEnabled(False)
        self.stop_btn.clicked.connect(self.stop_processes)
        layout.addWidget(self.stop_btn, alignment=Qt.AlignCenter)

        self.setLayout(layout)
        self.start_mapping()

    def start_mapping(self):
        # 启动第一个终端
        self.proc1 = QProcess(self)
        self.proc1.setProcessChannelMode(QProcess.MergedChannels)
        self.proc1.readyReadStandardOutput.connect(self.update_terminal1)
        self.proc1.started.connect(lambda: self.terminal1.append("建图进程已启动..."))
        self.proc1.finished.connect(self.proc1_done)
        self.proc1.start("bash", ["-c", 
            f"source /opt/ros/humble/setup.sh && "
            f"source {str(PATH_ROOT)}/install/setup.sh && ros2 launch map_manager realtime_fast_lio2.launch.py"
        ])

        # 启动第二个终端
        if args.debug:
            self.proc2 = QProcess(self)
            self.proc2.setProcessChannelMode(QProcess.MergedChannels)
            self.proc2.readyReadStandardOutput.connect(self.update_terminal2)
            self.proc2.started.connect(lambda: self.terminal2.append("回放进程已启动..."))
            self.proc2.finished.connect(self.proc2_done)
            self.proc2.start("bash", ["-c",
                f"source /opt/ros/humble/setup.sh && "
                f"source {str(PATH_ROOT)}/install/setup.sh && ros2 bag play /ws_618/data/rosbags/rosbag2_2025_06_14-11_22_50_build-2d-map-lab"
            ])

        self.stop_btn.setEnabled(True)

    def update_terminal1(self):
        data = self.proc1.readAllStandardOutput().data().decode()
        self.terminal1.moveCursor(self.terminal1.textCursor().End)
        self.terminal1.insertPlainText(data)

    def update_terminal2(self):
        data = self.proc2.readAllStandardOutput().data().decode()
        self.terminal2.moveCursor(self.terminal2.textCursor().End)
        self.terminal2.insertPlainText(data)

    def stop_processes(self):
        if self.proc1 and self.proc1.state() == QProcess.Running:
            os.kill(self.proc1.pid(), signal.SIGINT)
            QTimer.singleShot(1000, lambda: self.proc1.kill())
        if self.proc2 and self.proc2.state() == QProcess.Running:
            os.kill(self.proc2.pid(), signal.SIGINT)
            QTimer.singleShot(1000, lambda: self.proc2.kill())
        self.stop_btn.setEnabled(False)
        self.terminal1.append("已请求停止建图进程")
        self.terminal2.append("已请求停止回放进程")
        self.handle_save_and_copy()

    def proc1_done(self):
        self.proc1_finished = True

    def proc2_done(self):
        self.proc2_finished = True

    def handle_save_and_copy(self):
        save_dir = str(PATH_ROOT / "src/FAST_LIO/save_data")
        path_file = os.path.join(save_dir, "path.csv")
        scans_file = os.path.join(save_dir, "scans.pcd")
        time.sleep(1)
        if not (os.path.exists(path_file) and os.path.exists(scans_file)):
            QMessageBox.warning(self, "保存失败", "未找到 path.csv 或 scans.pcd 文件")
            return
        dialog = RenameDialog("新地图", self)
        while True:
            if dialog.exec_() == QDialog.Accepted:
                map_name = dialog.get_new_name()
                if not map_name:
                    continue
                maps_dir = str(PATH_ROOT / "src/map_manager/maps")
                new_map_dir = os.path.join(maps_dir, map_name)
                if os.path.exists(new_map_dir):
                    QMessageBox.warning(self, "重命名失败", "该名称已存在")
                    continue
                os.makedirs(new_map_dir)
                shutil.copy(path_file, os.path.join(new_map_dir, "path.csv"))
                shutil.copy(scans_file, os.path.join(new_map_dir, "scans.pcd"))
                QMessageBox.information(self, "保存成功", f"地图已保存到 {new_map_dir}")
                break
            else:
                break

class Convert2DParamDialog(QDialog):
    def __init__(self, min_pz_gain=0.5, max_pz_gain=1.5, grid_value_threshold=50.0, parent=None):
        super().__init__(parent)
        self.setWindowTitle("设置转2D参数")
        self.setFixedSize(350, 200)
        layout = QVBoxLayout()

        self.min_pz_gain_edit = QLineEdit(str(min_pz_gain), self)
        self.max_pz_gain_edit = QLineEdit(str(max_pz_gain), self)
        self.grid_value_threshold_edit = QLineEdit(str(grid_value_threshold), self)

        layout.addWidget(QLabel("min_pz_gain:"))
        layout.addWidget(self.min_pz_gain_edit)
        layout.addWidget(QLabel("max_pz_gain:"))
        layout.addWidget(self.max_pz_gain_edit)
        layout.addWidget(QLabel("grid_value_threshold:"))
        layout.addWidget(self.grid_value_threshold_edit)

        buttons = QDialogButtonBox(QDialogButtonBox.Ok | QDialogButtonBox.Cancel, self)
        buttons.accepted.connect(self.accept)
        buttons.rejected.connect(self.reject)
        layout.addWidget(buttons)
        self.setLayout(layout)

    def get_params(self):
        try:
            min_pz_gain = float(self.min_pz_gain_edit.text())
            max_pz_gain = float(self.max_pz_gain_edit.text())
            grid_value_threshold = float(self.grid_value_threshold_edit.text())
        except Exception:
            min_pz_gain, max_pz_gain, grid_value_threshold = 0.0, 0.5, 90.0
        return min_pz_gain, max_pz_gain, grid_value_threshold

class Convert2DDialog(QDialog):
    def __init__(self, map_name, min_pz_gain=0.0, max_pz_gain=0.5, grid_value_threshold=90.0, parent=None):
        super().__init__(parent)
        self.setWindowTitle("转2D终端")
        self.setFixedSize(800, 500)
        self.proc_build = None

        self.map_name = map_name
        self.min_pz_gain = min_pz_gain
        self.max_pz_gain = max_pz_gain
        self.grid_value_threshold = grid_value_threshold

        layout = QVBoxLayout()
        # 只显示build_2d_map终端
        self.terminal_build = QTextEdit(self)
        self.terminal_build.setReadOnly(True)
        layout.addWidget(QLabel("build_2d_map终端:"))
        layout.addWidget(self.terminal_build)

        self.stop_btn = QPushButton("停止", self)
        self.stop_btn.setEnabled(True)
        self.stop_btn.clicked.connect(self.stop_processes)
        layout.addWidget(self.stop_btn, alignment=Qt.AlignCenter)

        self.setLayout(layout)
        self.start_convert(map_name)

    def start_convert(self, map_name):
        # 只运行build_2d_map，不运行rviz2
        build_cmd = (
            f"source /opt/ros/humble/setup.sh && "
            f"source {str(PATH_ROOT)}/install/setup.sh && ros2 launch map_manager build_2d_map.launch.py "
            f"pcl_file_path:=maps/{map_name}/scans.pcd "
            f"path_file_path:=maps/{map_name}/path.csv "
            f"min_pz_gain:={self.min_pz_gain} max_pz_gain:={self.max_pz_gain} "
            f"grid_value_threshold:={self.grid_value_threshold}"
        )
        self.proc_build = QProcess(self)
        self.proc_build.setProcessChannelMode(QProcess.MergedChannels)
        self.proc_build.readyReadStandardOutput.connect(self.update_terminal_build)
        self.proc_build.started.connect(lambda: self.terminal_build.append("build_2d_map进程已启动..."))
        self.proc_build.start("bash", ["-c", build_cmd])

    def update_terminal_build(self):
        data = self.proc_build.readAllStandardOutput().data().decode()
        self.terminal_build.moveCursor(self.terminal_build.textCursor().End)
        self.terminal_build.insertPlainText(data)

    def stop_processes(self):
        if self.proc_build and self.proc_build.state() == QProcess.Running:
            os.kill(self.proc_build.pid(), signal.SIGINT)
            QTimer.singleShot(1000, lambda: self.proc_build.kill())
        self.stop_btn.setEnabled(False)
        self.terminal_build.append("已请求停止build_2d_map进程")

class NavTxtSelectDialog(QDialog):
    def __init__(self, folder, parent=None):
        super().__init__(parent)
        self.setWindowTitle("选择导航目标txt文件")
        self.setFixedSize(400, 400)
        self.selected_txt = None

        layout = QVBoxLayout()
        layout.addWidget(QLabel("请选择目标txt文件（可不选）："))

        self.list_widget = QListWidget(self)
        folder_path = os.path.join(parent.maps_dir, folder)
        txt_files = [f for f in os.listdir(folder_path) if f.lower().endswith('.txt')]
        for f in txt_files:
            item = QListWidgetItem(f)
            self.list_widget.addItem(item)
        self.list_widget.itemClicked.connect(self.on_item_clicked)
        layout.addWidget(self.list_widget)

        self.button_box = QDialogButtonBox(QDialogButtonBox.Ok | QDialogButtonBox.Cancel, self)
        self.button_box.accepted.connect(self.accept)
        self.button_box.rejected.connect(self.reject)
        layout.addWidget(self.button_box)
        self.setLayout(layout)

        # 默认选中第一个txt文件并设置为selected_txt
        if txt_files:
            self.list_widget.setCurrentRow(0)
            self.selected_txt = txt_files[0]

        # 取消选择功能：点击空白处取消选中
        self.list_widget.viewport().installEventFilter(self)
        # self.list_widget.clearSelection()  # 注释掉这一行，避免清除默认选中

    def eventFilter(self, obj, event):
        if obj == self.list_widget.viewport():
            if event.type() == event.MouseButtonPress:
                item = self.list_widget.itemAt(event.pos())
                if item is None:
                    self.list_widget.clearSelection()
                    self.selected_txt = None
        return super().eventFilter(obj, event)

    def on_item_clicked(self, item):
        self.selected_txt = item.text()

    def get_selected_txt(self):
        return self.selected_txt

class NavigationDialog(QDialog):
    def __init__(self, folder, png_file, txt_file=None, parent=None):
        super().__init__(parent)
        self.setWindowTitle("导航终端")
        self.setFixedSize(900, 600)
        self.proc_nav = None
        self.proc_bag = None

        layout = QVBoxLayout()

        # 上方导航终端
        self.terminal_nav = QTextEdit(self)
        self.terminal_nav.setReadOnly(True)
        layout.addWidget(QLabel("导航终端:"))
        layout.addWidget(self.terminal_nav)

        # 下方bag回放终端
        self.terminal_bag = QTextEdit(self)
        self.terminal_bag.setReadOnly(True)
        layout.addWidget(QLabel("bag回放终端:"))
        layout.addWidget(self.terminal_bag)

        self.stop_btn = QPushButton("停止", self)
        self.stop_btn.setEnabled(True)
        self.stop_btn.clicked.connect(self.stop_processes)
        layout.addWidget(self.stop_btn, alignment=Qt.AlignCenter)

        self.setLayout(layout)
        self.start_navigation(folder, png_file, txt_file)

    def start_navigation(self, folder, png_file, txt_file):
        # 导航终端
        if txt_file:
            nav_cmd = (
                f"source /opt/ros/humble/setup.sh && "
                f"source {str(PATH_ROOT)}/install/setup.sh && "
                f"ros2 launch navigation nav.launch.py "
                f"map_file_path:=maps/{folder}/{png_file} "
                f"targets_file_path:=maps/{folder}/{txt_file}"
            )
        else:
            nav_cmd = (
                f"source /opt/ros/humble/setup.sh && "
                f"source {str(PATH_ROOT)}/install/setup.sh && "
                f"ros2 launch navigation nav.launch.py "
                f"map_file_path:=maps/{folder}/{png_file}"
            )
        self.proc_nav = QProcess(self)
        self.proc_nav.setProcessChannelMode(QProcess.MergedChannels)
        self.proc_nav.readyReadStandardOutput.connect(self.update_terminal_nav)
        self.proc_nav.started.connect(lambda: self.terminal_nav.append("导航进程已启动..."))
        self.proc_nav.finished.connect(lambda: self.terminal_nav.append("导航进程已结束"))
        self.proc_nav.start("bash", ["-c", nav_cmd])

        # bag回放终端
        bag_cmd = (
            f"source /opt/ros/humble/setup.sh && "
            f"source {str(PATH_ROOT)}/install/setup.sh && "
            "ros2 bag play /ws_618/data/rosbags/rosbag2_2025_06_14-11_22_50_build-2d-map-lab"
        )
        self.proc_bag = QProcess(self)
        self.proc_bag.setProcessChannelMode(QProcess.MergedChannels)
        self.proc_bag.readyReadStandardOutput.connect(self.update_terminal_bag)
        self.proc_bag.started.connect(lambda: self.terminal_bag.append("bag回放进程已启动..."))
        self.proc_bag.finished.connect(lambda: self.terminal_bag.append("bag回放进程已结束"))
        self.proc_bag.start("bash", ["-c", bag_cmd])

    def update_terminal_nav(self):
        data = self.proc_nav.readAllStandardOutput().data().decode()
        self.terminal_nav.moveCursor(self.terminal_nav.textCursor().End)
        self.terminal_nav.insertPlainText(data)

    def update_terminal_bag(self):
        data = self.proc_bag.readAllStandardOutput().data().decode()
        self.terminal_bag.moveCursor(self.terminal_bag.textCursor().End)
        self.terminal_bag.insertPlainText(data)

    def stop_processes(self):
        if self.proc_nav and self.proc_nav.state() == QProcess.Running:
            os.kill(self.proc_nav.pid(), signal.SIGINT)
            QTimer.singleShot(1000, lambda: self.proc_nav.kill())
        if self.proc_bag and self.proc_bag.state() == QProcess.Running:
            os.kill(self.proc_bag.pid(), signal.SIGINT)
            QTimer.singleShot(1000, lambda: self.proc_bag.kill())
        self.stop_btn.setEnabled(False)
        self.terminal_nav.append("已请求停止导航进程")
        self.terminal_bag.append("已请求停止bag回放进程")

class NavVisSelectDialog(QDialog):
    """用于导航可视化的png和txt文件选择对话框"""
    def __init__(self, folder, maps_dir, parent=None):
        super().__init__(parent)
        self.setWindowTitle("选择可视化的PNG和TXT文件")
        self.setFixedSize(400, 400)
        self.selected_png = None
        self.selected_txt = None

        layout = QVBoxLayout()
        layout.addWidget(QLabel("请选择2D地图（png）文件："))
        self.png_list = QListWidget(self)
        folder_path = os.path.join(maps_dir, folder)
        png_files = [f for f in os.listdir(folder_path) if f.lower().endswith('.png')]
        for f in png_files:
            item = QListWidgetItem(f)
            self.png_list.addItem(item)
        self.png_list.itemClicked.connect(self.on_png_clicked)
        layout.addWidget(self.png_list)

        layout.addWidget(QLabel("请选择路径（txt）文件："))
        self.txt_list = QListWidget(self)
        txt_files = [f for f in os.listdir(folder_path) if f.lower().endswith('.txt')]
        for f in txt_files:
            item = QListWidgetItem(f)
            self.txt_list.addItem(item)
        self.txt_list.itemClicked.connect(self.on_txt_clicked)
        layout.addWidget(self.txt_list)

        self.button_box = QDialogButtonBox(QDialogButtonBox.Ok | QDialogButtonBox.Cancel, self)
        self.button_box.accepted.connect(self.accept)
        self.button_box.rejected.connect(self.reject)
        layout.addWidget(self.button_box)
        self.setLayout(layout)

    def on_png_clicked(self, item):
        self.selected_png = item.text()

    def on_txt_clicked(self, item):
        self.selected_txt = item.text()

    def get_selected(self):
        return self.selected_png, self.selected_txt

class SimpleWindow(QWidget):
    def __init__(self):
        super().__init__()
        self.maps_dir = str(PATH_ROOT / "src/map_manager/maps")
        self.selected_folder = None
        self.selected_png = None
        self.selected_txt = None
        self.init_ui()

    def init_ui(self):
        self.setWindowTitle("map_manager_gui")
        self.setGeometry(100, 100, 1200, 800)
        self.setStyleSheet("""
            QWidget {
                background-color: #f7f7fa;
            }
            QLabel#TitleLabel {
                color: #2d3a4b;
                font-weight: bold;
            }
            QPushButton {
                background-color: #4f8cff;
                color: white;
                border-radius: 6px;
                padding: 8px 20px;
                font-size: 15px;
            }
            QPushButton:pressed {
                background-color: #2566d6;
            }
            QScrollArea, QTextEdit {
                background: #fff;
                border-radius: 6px;
            }
        """)

        main_layout = QVBoxLayout()
        main_layout.setContentsMargins(30, 20, 30, 20)
        main_layout.setSpacing(10)

        # 中部内容布局
        content_layout = QHBoxLayout()
        content_layout.setSpacing(0)

        # 左侧：文件夹列表
        left_layout = QVBoxLayout()
        left_layout.setAlignment(Qt.AlignTop)
        left_layout.setSpacing(20)

        self.title_label = QLabel("地图名", self)
        self.title_label.setObjectName("TitleLabel")
        self.title_label.setFont(QFont("WenQuanYi Zen Hei", 18))
        left_layout.addWidget(self.title_label)

        self.folder_area = QScrollArea(self)
        self.folder_area.setWidgetResizable(True)
        self.folder_area.setFixedWidth(200)
        folder_widget = QFrame()
        self.folder_layout = QVBoxLayout(folder_widget)
        self.folder_layout.setAlignment(Qt.AlignTop)
        self.folder_area.setWidget(folder_widget)
        left_layout.addWidget(self.folder_area, stretch=1)

        content_layout.addLayout(left_layout, 0)

        # 文件名显示区域，分为上下两部分
        filelist_area_container = QVBoxLayout()
        filelist_area_container.setSpacing(10)

        # 上半部分：2D地图（png）
        self.png_label = QLabel("2D地图", self)
        self.png_label.setFont(QFont("WenQuanYi Zen Hei", 13, QFont.Bold))
        filelist_area_container.addWidget(self.png_label)

        self.png_filelist_area = QScrollArea(self)
        self.png_filelist_area.setWidgetResizable(True)
        self.png_filelist_area.setFixedHeight(180)
        png_filelist_widget = QFrame()
        self.png_filelist_layout = QVBoxLayout(png_filelist_widget)
        self.png_filelist_layout.setAlignment(Qt.AlignTop)
        self.png_filelist_area.setWidget(png_filelist_widget)
        filelist_area_container.addWidget(self.png_filelist_area)

        # 下半部分：路径文件（txt）
        self.txt_label = QLabel("路径文件", self)
        self.txt_label.setFont(QFont("WenQuanYi Zen Hei", 13, QFont.Bold))
        filelist_area_container.addWidget(self.txt_label)

        self.txt_filelist_area = QScrollArea(self)
        self.txt_filelist_area.setWidgetResizable(True)
        self.txt_filelist_area.setFixedHeight(180)
        txt_filelist_widget = QFrame()
        self.txt_filelist_layout = QVBoxLayout(txt_filelist_widget)
        self.txt_filelist_layout.setAlignment(Qt.AlignTop)
        self.txt_filelist_area.setWidget(txt_filelist_widget)
        filelist_area_container.addWidget(self.txt_filelist_area)

        # 填充剩余空间
        filelist_area_container.addStretch()
        content_layout.addLayout(filelist_area_container, 0)

        # 图片显示区域
        self.image_label = QLabel(self)
        self.image_label.setAlignment(Qt.AlignLeft | Qt.AlignTop)
        # 调整图片显示区域尺寸，适当缩小
        self.image_label.setMinimumSize(500, 500)
        self.image_label.setMaximumSize(600, 600)
        self.image_label.setStyleSheet("background: #fff; border: 1px solid #e0e0e0; border-radius: 8px;")
        content_layout.addWidget(self.image_label, 1)

        main_layout.addLayout(content_layout, 1)

        # 底部按钮布局
        btn_width = 120
        btn_height = 40
        btn_layout = QHBoxLayout()
        btn_layout.addStretch()
        self.mapping_btn = QPushButton("建图", self)
        self.mapping_btn.setFont(QFont("WenQuanYi Zen Hei", 14))
        self.mapping_btn.setFixedSize(btn_width, btn_height)
        self.mapping_btn.clicked.connect(self.open_mapping_dialog)
        btn_layout.addWidget(self.mapping_btn)

        self.rename_btn = QPushButton("重命名", self)
        self.rename_btn.setFont(QFont("WenQuanYi Zen Hei", 14))
        self.rename_btn.setFixedSize(btn_width, btn_height)
        self.rename_btn.clicked.connect(self.rename_folder)
        btn_layout.addWidget(self.rename_btn)

        self.delete_btn = QPushButton("删除", self)
        self.delete_btn.setFont(QFont("WenQuanYi Zen Hei", 14))
        self.delete_btn.setFixedSize(btn_width, btn_height)
        self.delete_btn.clicked.connect(self.delete_selected)
        btn_layout.addWidget(self.delete_btn)

        self.convert2d_btn = QPushButton("转2D", self)
        self.convert2d_btn.setFont(QFont("WenQuanYi Zen Hei", 14))
        self.convert2d_btn.setFixedSize(btn_width, btn_height)
        self.convert2d_btn.clicked.connect(self.convert_2d)
        btn_layout.addWidget(self.convert2d_btn)

        self.nav_btn = QPushButton("导航", self)
        self.nav_btn.setFont(QFont("WenQuanYi Zen Hei", 14))
        self.nav_btn.setFixedSize(btn_width, btn_height)
        self.nav_btn.clicked.connect(self.start_navigation_flow)
        btn_layout.addWidget(self.nav_btn)

        # 新增抓取按钮
        self.grasp_btn = QPushButton("抓取", self)
        self.grasp_btn.setFont(QFont("WenQuanYi Zen Hei", 14))
        self.grasp_btn.setFixedSize(btn_width, btn_height)
        self.grasp_btn.clicked.connect(self.launch_mani_gui)
        btn_layout.addWidget(self.grasp_btn)

        btn_layout.addStretch()
        main_layout.addLayout(btn_layout, 0)

        self.setLayout(main_layout)
        self.load_folders()

    def open_mapping_dialog(self):
        # 打开建图对话框为非模态，允许主界面继续可交互
        self.mapping_dialog = MappingDialog(self)
        # 当建图对话框关闭后，刷新地图列表
        try:
            self.mapping_dialog.finished.connect(lambda _: self.load_folders())
        except Exception:
            pass
        self.mapping_dialog.show()

    def load_folders(self):
        # 清空旧按钮
        while self.folder_layout.count():
            child = self.folder_layout.takeAt(0)
            if child.widget():
                child.widget().deleteLater()

        if not os.path.isdir(self.maps_dir):
            return

        for folder in sorted(os.listdir(self.maps_dir)):
            folder_path = os.path.join(self.maps_dir, folder)
            if not folder.startswith('nav_vis') :
                if os.path.isdir(folder_path):
                    btn = QPushButton(folder, self)
                    btn.setFont(QFont("WenQuanYi Zen Hei", 12))
                    btn.setCheckable(True)
                    btn.setStyleSheet("""
                        QPushButton {
                            background-color: #e6f0ff;
                            color: #2d3a4b;
                            border-radius: 6px;
                            padding: 6px 10px;
                            font-size: 14px;
                            text-align: left;
                        }
                        QPushButton:checked {
                            background-color: #4f8cff;
                            color: white;
                        }
                    """)
                    btn.clicked.connect(lambda checked, f=folder, b=btn: self.select_folder(f, b))
                    self.folder_layout.addWidget(btn)

    def select_folder(self, folder, btn):
        # 取消其他按钮的选中状态
        for i in range(self.folder_layout.count()):
            w = self.folder_layout.itemAt(i).widget()
            if isinstance(w, QPushButton):
                w.setChecked(False)
        btn.setChecked(True)
        self.selected_folder = folder
        self.selected_png = None
        self.show_file_list(folder)      # 先刷新文件列表和按钮
        self.show_folder_images(folder)  # 再自动高亮第一个png

    def delete_selected(self):
        # 如果选中了png文件则删除png，否则删除文件夹
        if hasattr(self, 'selected_png') and self.selected_png and self.selected_folder:
            folder_path = os.path.join(self.maps_dir, self.selected_folder)
            file_path = os.path.join(folder_path, self.selected_png)
            reply = QMessageBox.question(self, "确认删除", f"确定要删除图片文件 '{self.selected_png}' 吗？", QMessageBox.Yes | QMessageBox.No)
            if reply == QMessageBox.Yes:
                try:
                    os.remove(file_path)
                    self.show_file_list(self.selected_folder)
                    self.selected_png = None
                    # 如果当前显示的图片就是被删除的，清空图片显示
                    self.image_label.clear()
                    self.image_label.setText("图片已删除")
                except Exception as e:
                    QMessageBox.warning(self, "删除失败", f"删除图片失败: {e}")
        elif self.selected_folder:
            folder_path = os.path.join(self.maps_dir, self.selected_folder)
            reply = QMessageBox.question(self, "确认删除", f"确定要删除地图文件夹 '{self.selected_folder}' 吗？", QMessageBox.Yes | QMessageBox.No)
            if reply == QMessageBox.Yes:
                try:
                    shutil.rmtree(folder_path)
                    self.selected_folder = None
                    self.selected_png = None
                    self.load_folders()
                    self.show_file_list("")  # 清空文件列表
                    self.image_label.clear()
                    self.image_label.setText("已删除地图文件夹")
                except Exception as e:
                    QMessageBox.warning(self, "删除失败", f"删除文件夹失败: {e}")

    def show_file_list(self, folder):
        # 清空旧文件名
        self.selected_png = None
        self.selected_txt = None
        # 清空png部分
        if hasattr(self, "png_filelist_layout"):
            while self.png_filelist_layout.count():
                child = self.png_filelist_layout.takeAt(0)
                if child.widget():
                    child.widget().deleteLater()
        # 清空txt部分
        if hasattr(self, "txt_filelist_layout"):
            while self.txt_filelist_layout.count():
                child = self.txt_filelist_layout.takeAt(0)
                if child.widget():
                    child.widget().deleteLater()
        folder_path = os.path.join(self.maps_dir, folder)
        if not os.path.isdir(folder_path):
            return
        files = sorted(os.listdir(folder_path))
        png_btns = []
        txt_btns = []
        for fname in files:
            if fname.lower().endswith('.png'):
                btn = QPushButton(fname, self)
                btn.setFont(QFont("WenQuanYi Zen Hei", 11))
                btn.setStyleSheet(self.get_file_btn_style(fname, False))
                btn.clicked.connect(lambda checked, f=fname: self.select_png_file(folder, f))
                row_layout = QHBoxLayout()
                row_layout.addWidget(btn)
                row_layout.addStretch()
                row_widget = QFrame()
                row_widget.setLayout(row_layout)
                self.png_filelist_layout.addWidget(row_widget)
                png_btns.append((fname, btn))
            elif fname.lower().endswith('.txt'):
                btn = QPushButton(fname, self)
                btn.setFont(QFont("WenQuanYi Zen Hei", 11))
                btn.setStyleSheet(self.get_file_btn_style(fname, False))
                btn.clicked.connect(lambda checked, f=fname: self.select_txt_file(folder, f))
                row_layout = QHBoxLayout()
                row_layout.addWidget(btn)
                row_layout.addStretch()
                row_widget = QFrame()
                row_widget.setLayout(row_layout)
                self.txt_filelist_layout.addWidget(row_widget)
                txt_btns.append((fname, btn))
        self.filelist_btns = png_btns + txt_btns

    def select_png_file(self, folder, fname):
        self.selected_png = fname
        self.show_specific_image(folder, fname)
        self.highlight_png_in_filelist(fname)
        # 取消txt高亮（用户切换png时需重新选择txt）
        self.selected_txt = None
        self.highlight_txt_in_filelist(None)

    def select_txt_file(self, folder, fname):
        self.selected_txt = fname
        self.highlight_txt_in_filelist(fname)
        # 如果已高亮png文件，则自动调用导航可视化
        if self.selected_png:
            self.auto_visualize_navigation(folder, self.selected_png, fname)

    def auto_visualize_navigation(self, folder, png_file, txt_file):
        folder_path = os.path.join(self.maps_dir, folder)
        png_path = os.path.join(folder_path, png_file)
        txt_path = os.path.join(folder_path, txt_file)
        nav_vis_dir = os.path.join(self.maps_dir, "nav_vis")
        if not os.path.exists(nav_vis_dir):
            os.makedirs(nav_vis_dir)
        save_name = f"{Path(png_file).stem}_{Path(txt_file).stem}.png"
        save_path = os.path.join(nav_vis_dir, save_name)

        # 动态导入mytest_cv_draw_point.py并调用run
        # module_path = os.path.join(os.path.dirname(__file__), "mytest_cv_draw_point.py")
        # spec = importlib.util.spec_from_file_location("mytest_cv_draw_point", module_path)
        # mytest_cv_draw_point = importlib.util.module_from_spec(spec)
        # spec.loader.exec_module(mytest_cv_draw_point)

        # self.run_and_save_img(mytest_cv_draw_point, png_path, txt_path, save_path)
        # if os.path.exists(save_path):
        #     pixmap = QPixmap(save_path)
        #     label_width = self.image_label.width()
        #     label_height = self.image_label.height()
        #     scaled_pixmap = pixmap.scaled(
        #         label_width,
        #         label_height,
        #         Qt.KeepAspectRatio,
        #         Qt.SmoothTransformation
        #     )
        #     self.image_label.setPixmap(scaled_pixmap)
        # else:
        #     QMessageBox.warning(self, "导航可视化", "图片生成失败，请检查输入文件。")

    def highlight_png_in_filelist(self, fname):
        # 高亮选中的png文件名
        if not hasattr(self, "filelist_btns"):
            return
        for f, btn in self.filelist_btns:
            if f == fname:
                btn.setStyleSheet(self.get_file_btn_style(f, True))
            else:
                btn.setStyleSheet(self.get_file_btn_style(f, False))

    def highlight_txt_in_filelist(self, fname):
        # 高亮选中的txt文件名
        if not hasattr(self, "filelist_btns"):
            return
        for f, btn in self.filelist_btns:
            if f.lower().endswith('.txt'):
                if f == fname:
                    btn.setStyleSheet("color: #fff; background: #4f8cff; border-radius: 4px; padding: 2px 8px; text-align: left;")
                else:
                    btn.setStyleSheet(self.get_file_btn_style(f, False))

    def get_file_btn_style(self, fname, highlight):
        if fname.lower().endswith('.png'):
            if highlight:
                return "color: #fff; background: #4f8cff; border-radius: 4px; padding: 2px 8px; text-align: left;"
            else:
                return "color: #2d3a4b; background: #f0f4fa; border-radius: 4px; padding: 2px 8px; text-align: left;"
        else:
            return "color: #2d3a4b; background: #f0f4fa; border-radius: 4px; padding: 2px 8px; text-align: left;"

    def show_specific_image(self, folder, fname):
        folder_path = os.path.join(self.maps_dir, folder)
        img_path = os.path.join(folder_path, fname)
        if os.path.exists(img_path):
            pixmap = QPixmap(img_path)
            # 获取image_label的显示区域大小
            label_width = self.image_label.width()
            label_height = self.image_label.height()
            # 按label区域等比例最大化显示
            scaled_pixmap = pixmap.scaled(
                label_width,
                label_height,
                Qt.KeepAspectRatio,
                Qt.SmoothTransformation
            )
            self.image_label.setPixmap(scaled_pixmap)
        else:
            self.image_label.clear()
            self.image_label.setText("图片不存在")

    def show_folder_images(self, folder):
        # 默认自动选中并显示第一个png文件，相当于自动点击2D地图列表下的第一个png文件
        folder_path = os.path.join(self.maps_dir, folder)
        png_files = [f for f in os.listdir(folder_path) if f.lower().endswith('.png')]
        if png_files:
            # 自动调用select_png_file，等价于用户点击了第一个png文件
            self.select_png_file(folder, png_files[0])
        else:
            self.image_label.clear()
            self.image_label.setText("未进行转2D操作")
            self.selected_png = None
            self.highlight_png_in_filelist(None)

    def rename_folder(self):
        if not self.selected_folder:
            return
        while True:
            dialog = RenameDialog(self.selected_folder, self)
            if dialog.exec_() == QDialog.Accepted:
                new_name = dialog.get_new_name()
                if not new_name or new_name == self.selected_folder:
                    break
                old_path = os.path.join(self.maps_dir, self.selected_folder)
                new_path = os.path.join(self.maps_dir, new_name)
                if not os.path.exists(new_path):
                    os.rename(old_path, new_path)
                    self.selected_folder = new_name
                    self.load_folders()
                    break
                else:
                    QMessageBox.warning(self, "重命名失败", "该名称已存在")
            else:
                break

    def convert_2d(self):
        if not self.selected_folder:
            QMessageBox.warning(self, "未选择地图", "请先选择一个地图文件夹")
            return
        # 参数输入对话框
        param_dialog = Convert2DParamDialog()
        if param_dialog.exec_() == QDialog.Accepted:
            min_pz_gain, max_pz_gain, grid_value_threshold = param_dialog.get_params()
            # 使用非模态对话框显示转2D终端，避免阻塞主窗口
            self.convert2d_dialog = Convert2DDialog(
                self.selected_folder,
                min_pz_gain=min_pz_gain,
                max_pz_gain=max_pz_gain,
                grid_value_threshold=grid_value_threshold,
                parent=self
            )
            self.convert2d_dialog.show()

    def start_navigation_flow(self):
        if not self.selected_folder or not self.selected_png or not self.selected_png.lower().endswith('.png'):
            QMessageBox.warning(self, "导航前提", "请先选择地图文件夹并选择一个PNG地图文件")
            return
        # 弹出txt选择对话框
        txt_dialog = NavTxtSelectDialog(self.selected_folder, parent=self)
        txt_file = None
        if txt_dialog.exec_() == QDialog.Accepted:
            txt_file = txt_dialog.get_selected_txt()
            # 以非模态方式打开导航终端窗口，保持主界面可交互
            self.nav_dialog = NavigationDialog(self.selected_folder, self.selected_png, txt_file, parent=self)
            self.nav_dialog.show()

    def visualize_navigation_dialog(self):
        # 选择文件夹后弹出选择png和txt的对话框
        if not self.selected_folder:
            QMessageBox.information(self, "提示", "请先选择地图文件夹")
            return
        dialog = NavVisSelectDialog(self.selected_folder, self.maps_dir, self)
        if dialog.exec_() == QDialog.Accepted:
            png_file, txt_file = dialog.get_selected()
            if not png_file or not txt_file:
                QMessageBox.warning(self, "提示", "请同时选择PNG和TXT文件")
                return
            folder_path = os.path.join(self.maps_dir, self.selected_folder)
            png_path = os.path.join(folder_path, png_file)
            txt_path = os.path.join(folder_path, txt_file)
            # 生成保存文件名：png文件名_txt文件名.png，保存在maps/nav_vis文件夹
            nav_vis_dir = os.path.join(self.maps_dir, "nav_vis")
            if not os.path.exists(nav_vis_dir):
                os.makedirs(nav_vis_dir)
            save_name = f"{Path(png_file).stem}_{Path(txt_file).stem}.png"
            save_path = os.path.join(nav_vis_dir, save_name)

            # # 动态导入mytest_cv_draw_point.py并调用run
            # module_path = os.path.join(os.path.dirname(__file__), "mytest_cv_draw_point.py")
            # spec = importlib.util.spec_from_file_location("mytest_cv_draw_point", module_path)
            # mytest_cv_draw_point = importlib.util.module_from_spec(spec)
            # spec.loader.exec_module(mytest_cv_draw_point)
            
            # self.run_and_save_img(mytest_cv_draw_point, png_path, txt_path, save_path)
            # if os.path.exists(save_path):
            #     # 直接在右侧显示生成的图片
            #     pixmap = QPixmap(save_path)
            #     label_width = self.image_label.width()
            #     label_height = self.image_label.height()
            #     scaled_pixmap = pixmap.scaled(
            #         label_width,
            #         label_height,
            #         Qt.KeepAspectRatio,
            #         Qt.SmoothTransformation
            #     )
            #     self.image_label.setPixmap(scaled_pixmap)
            # else:
            #     QMessageBox.warning(self, "导航可视化", "图片生成失败，请检查输入文件。")

    def run_and_save_img(self, module, png_path, txt_path, save_path):
        import cv2
        img = cv2.imread(png_path)
        from pathlib import Path
        height, width = img.shape[:2]
        x, y = Path(png_path).stem.split('_')[-2:]
        x, y = -int(x)//10, height+int(y)//10
        x2, y2 = x, y
        img = cv2.circle(img, (x, y), 3, (255, 0, 0), 3)
        with open(txt_path, "r") as f:
            for line in f:
                try:
                    a = float(line.rstrip().split(' ')[0])
                    b = float(line.rstrip().split(' ')[1])
                except Exception:
                    continue
                x1 = x - int(a * 10)
                y1 = y + int(b * 10)
                img = cv2.circle(img, (x1, y1), 2, (0, 255, 0), 2)
                img = cv2.line(img, (x2, y2), (x1, y1), (0, 0, 255), 1)
                x2, y2 = x1, y1
        cv2.imwrite(save_path, img)

    def launch_mani_gui(self):
        import subprocess
        import sys
        mani_path = os.path.join(os.path.dirname(__file__), "mani_gui.py")
        if os.path.exists(mani_path):
            cmd = (
                # "colcon build --symlink-install --packages-select manipulation gui && "
                "source install/setup.sh && "
                "ros2 run gui mani_gui.py"
            )
            subprocess.Popen(
                ["bash", "-c", cmd],
                cwd=str(PATH_ROOT)
            )
        else:
            QMessageBox.warning(self, "抓取", "未找到 mani_gui.py 文件")

if __name__ == "__main__":
    args = parse_args()
    app = QApplication(sys.argv)
    window = SimpleWindow()
    window.show()

    def cleanup_nav_vis():
        nav_vis_dir = os.path.join(window.maps_dir, "nav_vis")
        if os.path.exists(nav_vis_dir):
            for fname in os.listdir(nav_vis_dir):
                if fname.lower().endswith('.png'):
                    try:
                        os.remove(os.path.join(nav_vis_dir, fname))
                    except Exception:
                        pass

    app.aboutToQuit.connect(cleanup_nav_vis)
    sys.exit(app.exec_())